#coding:utf-8
from geopy.distance import vincenty
import numpy as np
from config import cfg
#from matplotlib import pyplot as plt
import pickle, time
from scipy.signal import argrelextrema
#from pymouse import PyMouse

outliers = []

def compute_dis_lat_lon(p1, p2):
    """
    compute distance between two (lat, lon) points
    p1 and p2 are tuples
    """
    return vincenty(p1, p2).meters

def _get_line_param(p1, p2):
    (x1, y1) = p1
    (x2, y2) = p2
    a = float(y1 - y2) / float(x1 - x2)
    b = y1 - a * x1
    return (a, b)

sign = lambda x: (1, -1)[x < 0]
def _get_points_on_line(p1, p2):    #航段分微元（逻辑bug）
    
    (x1, y1) = p1
    (x2, y2) = p2
    if x1 == x2:
        y = np.arange(y1, y2 + sign(y2-y1), sign(y2-y1))
        x = np.ones_like(y) * x1
    else:
        if abs(x2-x1)>=abs(y2-y1):
            x = np.arange(x1, x2 + sign(x2-x1), sign(x2-x1)) #bug,X过于接近，数组缩短，数值对比有问题
            a,b = _get_line_param(p1, p2)
            y = a * x + b
        else:
            y = np.arange(y1, y2 + sign(y2-y1), sign(y2-y1))
            a,b = _get_line_param(p1, p2)
            x =( y - b ) / a		
    x = np.round(x).astype(np.int)
    y = np.round(y).astype(np.int)
    return (x, y)

def _popup_outlier(local_max, p1, p2):
    """ Pop up outlier, outlier occurs when the max value of alt_line is above the line defined by p1 and p2"""
    a,b = _get_line_param(p1, p2)
    #diff = alt_line - (a*np.arange(p1[0], p2[0], 1) + b)
    y_local_max = a*local_max[0] + b
    alt_local_max = local_max[1]
    crossed = alt_local_max - y_local_max > 0
    if np.any(crossed):
        crossed_local_max = local_max[:,crossed]
        # choose max local_max
        out_i = crossed_local_max[0][crossed_local_max[1].argmax()]
        out_alt = crossed_local_max[1][crossed_local_max[1].argmax()]
        #out_i = local_max[0][alt_local_max.argmax()]
        #out_alt = alt_local_max.max()
        outliers.append((out_i, out_alt))
        local_max_left = local_max[:,local_max[0] <= out_i]
        local_max_right = local_max[:, local_max[0] >= out_i]
        _popup_outlier(local_max_left, p1, (out_i,out_alt))
        _popup_outlier(local_max_right, (out_i, out_alt), p2)
    else:
        return
    #if out_alt > (a*out_i + b):
        #outliers.append((out_i, out_alt))
        #_popup_outlier(alt_line[p1[0]:out_i+1], p1, (out_i,out_alt))
        #_popup_outlier(alt_line[out_i:p2[0]+1], (out_i, out_alt), p2)
        #return (max_alt_i + p1[0], max_alt)
        #return _popup_outlier(alt_line[p1:max_alt_i+1], p1, (max_alt_i,max_alt)) + \
        #       _popup_outlier(alt_line[max_alt_i:p2+1], (max_lat_i, max_alt), p2)
    #else:
        #return []



def get_val_on_straight_line(alt, p1, p2):
    """
    get altitude along a straight line, p1 (x1, y1) and p2 (x2, y2)
    y = ax + b
    """
    x, y = _get_points_on_line(p1, p2)
    alt_line = []
    for i in range(x.size):
        alt_line.append(alt[y[i],x[i]])
    return np.array(alt_line)

def get_road_point_on_line(alt, p1, p2):
    """Get road point on a line, given altitude and start(p1) and stop(p2) points"""
    alt_line = get_val_on_straight_line(alt, p1, p2)

def try_tail_num_x(num_x):
    m = PyMouse()
    tail_width = cfg.tail_rect[2] - cfg.tail_rect[0]
    print('sleep for 5 seconds')
    time.sleep(5)
    for n in range(num_x):
        #m.move(cfg.tail_rect[0], cfg.tail_rect[1])
        #time.sleep(2)
        m.move(cfg.tail_rect[2], cfg.tail_rect[1])
        time.sleep(2)
        # m.press(cfg.tail_rect[2], cfg.tail_rect[1])
        # time.sleep(2)
        # m.move(cfg.tail_rect[0], cfg.tail_rect[1])
        m.drag(cfg.tail_rect[0], cfg.tail_rect[1])
        time.sleep(2)
        m.release(cfg.tail_rect[0], cfg.tail_rect[1])
        time.sleep(2)

def try_tail_num_y(num_y):
    m = PyMouse()
    tail_height = cfg.tail_rect[3] - cfg.tail_rect[1]
    print('sleep for 5 seconds')
    time.sleep(5)
    for n in range(num_y):
        m.move(cfg.tail_rect[0], cfg.tail_rect[3])
        time.sleep(2)
        m.drag(cfg.tail_rect[0], cfg.tail_rect[1])
        time.sleep(2)
        m.release(cfg.tail_rect[0], cfg.tail_rect[1])
        time.sleep(2)

if __name__ == "__main__":
    map_file = 'map_%s_%s.pkl' % (cfg.from_, cfg.to_)
    lat, lon, alt, image_id, start_xy, stop_xy = pickle.load(open(map_file, 'rb'))
    alt_line = get_val_on_straight_line(alt, start_xy, stop_xy) + 75
    lat_line = get_val_on_straight_line(lat, start_xy, stop_xy)
    lon_line = get_val_on_straight_line(lon, start_xy, stop_xy)
    local_max = np.squeeze(argrelextrema(alt_line, np.greater))
    local_max_alt = alt_line[local_max]
    local_max += start_xy[0]
    local_max = np.array([local_max, local_max_alt])
    _popup_outlier(local_max, (start_xy[0], alt_line[0]), (stop_xy[0], alt_line[-1]))
    road_point = [(lat_line[0],lon_line[0],alt_line[0])]
    for o in outliers:
        x = int(o[0])
        lat_o = lat_line[x - start_xy[0]]
        lon_o = lon_line[x - start_xy[0]]
        road_point.append((lat_o, lon_o, o[1]))
    road_point.append((lat_line[0],lon_line[0],alt_line[0]))
    with open('road_point_%s_%s.txt' % (cfg.from_, cfg.to_), 'w') as f:
        for p in road_point:
            f.write('lat=%f, lon=%f, alt=%f\n' % (p[0],p[1],p[2]))

    #x = np.arange(start_xy[0], stop_xy[0],1)
    #plt.plot(x, alt_line)
    #plt.show()
